#include <iostream>
#include <pthread.h>
#include <malloc.h>
#include <time.h>
#include <stdlib.h>
#include <semaphore.h>
#include <xmmintrin.h> //SSE
#include <nmmintrin.h> //SSSE4.2

using namespace std;
const int NUM_THREADS1=2;
const int NUM_THREADS2=5;
const int NUM_THREADS3=8;
int n=1000;
float **a;
float **b1;
float **b2;
float **c2;
void starta(float** x, int n) {//初始化数组
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < i; j++)
            x[i][j] = 0;
        x[i][i] = 1.0;
        for (int j = i + 1; j < n; j++)
            x[i][j] = rand();
    }
    for (int k = 0; k < n; k++) {
        for (int i = k + 1; i < n; i++) {
            for (int j = 0; j < n; j++) {
                x[i][j] += x[k][j];
            }
        }
    }
}
void clone(float **A, float** B,int n)
{
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            B[i][j] = A[i][j];
        }
    }
}
void gaussionone(float** x, int n) {//高斯消元串行算法
    for (int k = 0; k < n; k++) {
        for (int j = k + 1; j < n; j++) {
            x[k][j] /= x[k][k];
        }
        x[k][k] = 1.0;
        for (int i = k + 1; i < n; i++) {
            for (int j = k + 1; j < n; j++) {
                x[i][j] -= x[i][k] * x[k][j];
            }
            x[i][k] = 0;
        }
    }
}
//静态线程+信号量版本
typedef struct{
    int t_id;//线程id
}threadParam_t1;
sem_t sem_main1;
sem_t sem_workerstart1[NUM_THREADS1];
sem_t sem_workerend1[NUM_THREADS1];
void* threadFunc1sse1(void* param)
{
    threadParam_t1* p = (threadParam_t1*)param;
    int t_id = p->t_id;
    for (int k = 0; k < n; k++)
    {
        sem_wait(&sem_workerstart1[t_id]);
        for (int i = k + t_id + 1; i < n; i +=NUM_THREADS1)
        {
            __m128 vaik = _mm_set1_ps(b1[i][k]);
            int j;
            for (j = k + 1; j + 4 <= n; j += 4)
            {
                __m128 vakj = _mm_loadu_ps(&b1[k][j]);
                __m128 vaij = _mm_loadu_ps(&b1[i][j]);
                __m128 vx = _mm_mul_ps(vakj, vaik);
                vaij = _mm_sub_ps(vaij, vx);
                _mm_storeu_ps(&b1[i][j], vaij);
            }
            for (; j < n; j++)
            {
                b1[i][j]-= b1[k][j] * b1[i][k];
            }
            b1[i][k] = 0;
        }
        sem_post(&sem_main1);
        sem_wait(&sem_workerend1[t_id]);
    }
    pthread_exit(NULL);
}
void gaussionquitesse1(float** x,int n)
{
    sem_init(&sem_main1, 0, 0);
    for (int i = 0; i <NUM_THREADS1; i++)
    {
        sem_init(&sem_workerstart1[i], 0, 0);
        sem_init(&sem_workerend1[i], 0, 0);
    }
    pthread_t handles[NUM_THREADS1];
    threadParam_t1 param[NUM_THREADS1];
    for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
    {
        param[t_id].t_id = t_id;
        pthread_create(&handles[t_id], NULL, threadFunc1sse1, (void*)&param[t_id]);
    }
    for (int k = 0; k < n; k++)
    {
        __m128 vt = _mm_set1_ps(x[k][k]);
        int j;
        for (j = k + 1; j + 4 <= n; j += 4)
        {
            __m128 va = _mm_loadu_ps(&x[k][j]);
            va = _mm_div_ps(va, vt);
            _mm_storeu_ps(&x[k][j], va);
        }
        for (; j < n; j++)
        {
            x[k][j] /= x[k][k];
        }
        x[k][k] = 1.0;
        for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
        {
            sem_post(&sem_workerstart1[t_id]);
        }
        for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
        {
            sem_wait(&sem_main1);
        }
        for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
        {
            sem_post(&sem_workerend1[t_id]);
        }
    }
    for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
    {
        pthread_join(handles[t_id], NULL);
    }
    sem_destroy(&sem_main1);
    for (int t_id = 0; t_id <NUM_THREADS1; t_id++)
    {
        sem_destroy(&sem_workerstart1[t_id]);
        sem_destroy(&sem_workerend1[t_id]);
    }
}

sem_t sem_main2;
sem_t sem_workerstart2[NUM_THREADS2];
sem_t sem_workerend2[NUM_THREADS2];
void* threadFunc1sse2(void* param)
{
    threadParam_t1* p = (threadParam_t1*)param;
    int t_id = p->t_id;
    for (int k = 0; k < n; k++)
    {
        sem_wait(&sem_workerstart2[t_id]);
        for (int i = k + t_id + 1; i < n; i +=NUM_THREADS2)
        {
            __m128 vaik = _mm_set1_ps(b2[i][k]);
            int j;
            for (j = k + 1; j + 4 <= n; j += 4)
            {
                __m128 vakj = _mm_loadu_ps(&b2[k][j]);
                __m128 vaij = _mm_loadu_ps(&b2[i][j]);
                __m128 vx = _mm_mul_ps(vakj, vaik);
                vaij = _mm_sub_ps(vaij, vx);
                _mm_storeu_ps(&b2[i][j], vaij);
            }
            for (; j < n; j++)
            {
                b2[i][j]-= b2[k][j] * b2[i][k];
            }
            b2[i][k] = 0;
        }
        sem_post(&sem_main2);
        sem_wait(&sem_workerend2[t_id]);
    }
    pthread_exit(NULL);
}
void gaussionquitesse2(float** x,int n)
{
    sem_init(&sem_main2, 0, 0);
    for (int i = 0; i <NUM_THREADS2; i++)
    {
        sem_init(&sem_workerstart2[i], 0, 0);
        sem_init(&sem_workerend2[i], 0, 0);
    }
    pthread_t handles[NUM_THREADS2];
    threadParam_t1 param[NUM_THREADS2];
    for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
    {
        param[t_id].t_id = t_id;
        pthread_create(&handles[t_id], NULL, threadFunc1sse2, (void*)&param[t_id]);
    }
    for (int k = 0; k < n; k++)
    {
        __m128 vt = _mm_set1_ps(x[k][k]);
        int j;
        for (j = k + 1; j + 4 <= n; j += 4)
        {
            __m128 va = _mm_loadu_ps(&x[k][j]);
            va = _mm_div_ps(va, vt);
            _mm_storeu_ps(&x[k][j], va);
        }
        for (; j < n; j++)
        {
            x[k][j] /= x[k][k];
        }
        x[k][k] = 1.0;
        for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
        {
            sem_post(&sem_workerstart2[t_id]);
        }
        for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
        {
            sem_wait(&sem_main2);
        }
        for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
        {
            sem_post(&sem_workerend2[t_id]);
        }
    }
    for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
    {
        pthread_join(handles[t_id], NULL);
    }
    sem_destroy(&sem_main2);
    for (int t_id = 0; t_id <NUM_THREADS2; t_id++)
    {
        sem_destroy(&sem_workerstart2[t_id]);
        sem_destroy(&sem_workerend2[t_id]);
    }
}

sem_t sem_main3;
sem_t sem_workerstart3[NUM_THREADS3];
sem_t sem_workerend3[NUM_THREADS3];
void* threadFunc1sse3(void* param)
{
    threadParam_t1* p = (threadParam_t1*)param;
    int t_id = p->t_id;
    for (int k = 0; k < n; k++)
    {
        sem_wait(&sem_workerstart3[t_id]);
        for (int i = k + t_id + 1; i < n; i +=NUM_THREADS3)
        {
            __m128 vaik = _mm_set1_ps(c2[i][k]);
            int j;
            for (j = k + 1; j + 4 <= n; j += 4)
            {
                __m128 vakj = _mm_loadu_ps(&c2[k][j]);
                __m128 vaij = _mm_loadu_ps(&c2[i][j]);
                __m128 vx = _mm_mul_ps(vakj, vaik);
                vaij = _mm_sub_ps(vaij, vx);
                _mm_storeu_ps(&c2[i][j], vaij);
            }
            for (; j < n; j++)
            {
                c2[i][j]-= c2[k][j] * c2[i][k];
            }
            c2[i][k] = 0;
        }
        sem_post(&sem_main3);
        sem_wait(&sem_workerend3[t_id]);
    }
    pthread_exit(NULL);
}
void gaussionquitesse3(float** x,int n)
{
    sem_init(&sem_main3, 0, 0);
    for (int i = 0; i <NUM_THREADS1; i++)
    {
        sem_init(&sem_workerstart3[i], 0, 0);
        sem_init(&sem_workerend3[i], 0, 0);
    }
    pthread_t handles[NUM_THREADS3];
    threadParam_t1 param[NUM_THREADS3];
    for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
    {
        param[t_id].t_id = t_id;
        pthread_create(&handles[t_id], NULL, threadFunc1sse3, (void*)&param[t_id]);
    }
    for (int k = 0; k < n; k++)
    {
        __m128 vt = _mm_set1_ps(x[k][k]);
        int j;
        for (j = k + 1; j + 4 <= n; j += 4)
        {
            __m128 va = _mm_loadu_ps(&x[k][j]);
            va = _mm_div_ps(va, vt);
            _mm_storeu_ps(&x[k][j], va);
        }
        for (; j < n; j++)
        {
            x[k][j] /= x[k][k];
        }
        x[k][k] = 1.0;
        for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
        {
            sem_post(&sem_workerstart3[t_id]);
        }
        for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
        {
            sem_wait(&sem_main3);
        }
        for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
        {
            sem_post(&sem_workerend3[t_id]);
        }
    }
    for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
    {
        pthread_join(handles[t_id], NULL);
    }
    sem_destroy(&sem_main3);
    for (int t_id = 0; t_id <NUM_THREADS3; t_id++)
    {
        sem_destroy(&sem_workerstart3[t_id]);
        sem_destroy(&sem_workerend3[t_id]);
    }
}
int main()
{
   for (int k = 100; k <2000; k += 100) {
    n = k;
    a=new float*[n];
    b1=new float*[n];
    b2=new float*[n];
    c2=new float*[n];
    for(int i=0;i<n;i++){
        a[i]=new float[n];
        b1[i]=new float[n];
        b2[i]=new float[n];
        c2[i]=new float[n];
    }
    starta(a,n);
    clone(a,b1,n);
    clone(a,b2,n);
    clone(a,c2,n);

    clock_t start2,finish2;
    start2 = clock();
    gaussionquitesse1(b1, n);
    finish2 = clock();
    cout <<n<<" 2 "<<(double)(finish2 - start2) * 1000 / CLOCKS_PER_SEC << "ms" << endl;

    clock_t start3,finish3;
    start3 = clock();
    gaussionquitesse2(b2, n);
    finish3 = clock();
    cout <<n<<" 5 "<<(double)(finish3 - start3) * 1000 / CLOCKS_PER_SEC << "ms" << endl;

    clock_t start5,finish5;
    start5 = clock();
    gaussionquitesse3(c2, n);
    finish5 = clock();
    cout <<n<<" 8 "<<(double)(finish5 - start5) * 1000 / CLOCKS_PER_SEC << "ms" << endl;

}
}
